#include "map_operation/map_operation.h"

MapOperation::MapOperation(ros::NodeHandle nh, std::string map_file, std::string param_file, std::string path)
: nh_(nh),
  map_file_(map_file),
  param_file_(param_file),
  file_path_(path)
{
    ptr_graph_ = std::make_shared<CommonLib::TopoGraph>();

    pub_vis_map_ = nh_.advertise<amr_trajectory_msgs::amr_trajectory_msg>("/nav_path", 10);
    ptr_points_.reset(new pcl::PointCloud<pcl::PointXYZL>);

    srv_path_info_  = nh_.advertiseService("/GetPathInfoSrv",  &MapOperation::GetPathInfoService,  this);
    srv_point_info_ = nh_.advertiseService("/GetPointInfoSrv", &MapOperation::GetPointInfoService, this);
    srv_nearest_point_ = nh_.advertiseService("/GetNearestInfoSrv", &MapOperation::GetNearestInfoService, this);
}

MapOperation::~MapOperation()
{
    v_path_.clear();
    all_trajectory_.points.clear();
    common_lib::SpdLog::Instance()->EndLog();
}

bool MapOperation::Init(void)
{
    common_lib::SpdLog::Instance()->InitSpdLog(file_path_+"/log", "map");
    // 加载地图文件
    ptr_graph_->LoadGraph(map_file_);
    printf("pathsize: %d, pointsize: %d\n", (int)(ptr_graph_->GetPathMap()).size(), (int)(ptr_graph_->GetPointMap()).size());
    LINFO("MapOperation(), pathsize: {}, pointsize: {}", (int)(ptr_graph_->GetPathMap()).size(), (int)(ptr_graph_->GetPointMap()).size());

    // 加载参数文件
    YAML::Node vehicle_params = YAML::LoadFile(param_file_);
    step_size_ = vehicle_params["step_size"].as<double>();

    AllMapInfo();
    AllTrajectoryInfo();
    CalcHeading(all_trajectory_);
    GenerateViewer();
    return true;
}

void MapOperation::AllMapInfo(void)
{
    v_path_.clear();
    for (auto path : ptr_graph_->GetPathMap())
    {
        amr_trajectory_msgs::path_info path_info;
        path_info.pathid = path.second->pathid;
        path_info.direction = "start2end";
        path.second->start.c_str();

        amr_trajectory_msgs::attrpoints point;
        point.pointid = path.second->start.c_str();
        point.pose.position.x = ptr_graph_->GetPointMap()[point.pointid]->x;
        point.pose.position.y = ptr_graph_->GetPointMap()[point.pointid]->y;
        point.pose.position.z = ptr_graph_->GetPointMap()[point.pointid]->z;
        path_info.control_points.push_back(point);

        point.pointid = path.second->end.c_str();
        point.pose.position.x = ptr_graph_->GetPointMap()[point.pointid]->x;
        point.pose.position.y = ptr_graph_->GetPointMap()[point.pointid]->y;
        point.pose.position.z = ptr_graph_->GetPointMap()[point.pointid]->z;
        path_info.control_points.push_back(point);

        //TODO 现在地图文件中不区分关键点和控制点，先相同。
        path_info.key_points = path_info.control_points;

        path_info.angle = path.second->amr_angle;
        path_info.pathtype = "line";
        path_info.max_speed = path.second->maxspeed;
        path_info.angle_type = path.second->angle_type;

        v_path_.push_back(path_info);
    }
}

void MapOperation::AllTrajectoryInfo(void)
{
    all_trajectory_.points.clear();
    for (auto pathinfo : v_path_)
    {
        if (um_path_.find(pathinfo.pathtype) != um_path_.end())
        {
            // printf("id: %s, size: %d\n", pathinfo.pathid.c_str(), (int)all_trajectory_.points.size());
            amr_trajectory_msgs::amr_trajectory_msg trajectory = (this->*um_path_[pathinfo.pathtype])(pathinfo, pathinfo.pathid);
            all_trajectory_.points.insert(all_trajectory_.points.end(), trajectory.points.begin(), trajectory.points.end());
        }
        else
        {
            throw("Not Support Line Pattern");
        }
    }
}

// 直线轨迹离散
amr_trajectory_msgs::amr_trajectory_msg MapOperation::LinePro(amr_trajectory_msgs::path_info  &path, std::string id)
{
    amr_trajectory_msgs::amr_trajectory_msg trajectory;
    if (path.control_points.size() == 2)
    {
        double dis = std::hypotf(path.control_points[0].pose.position.x-path.control_points[1].pose.position.x, path.control_points[0].pose.position.y-path.control_points[1].pose.position.y);

        int cnt = 0;
        amr_trajectory_msgs::point_info tmp_point;
        double s = fabs(path.control_points[1].pose.position.y - path.control_points[0].pose.position.y) / dis;
        double c = fabs(path.control_points[1].pose.position.x - path.control_points[0].pose.position.x) / dis;

        while (cnt * step_size_ < dis)
        {
            tmp_point.pathid = path.pathid;
            if (path.control_points[1].pose.position.x > path.control_points[0].pose.position.x)
                tmp_point.pose.position.x = path.control_points[0].pose.position.x + cnt * step_size_ * c;
            else
                tmp_point.pose.position.x = path.control_points[0].pose.position.x - cnt * step_size_ * c;

            if (path.control_points[1].pose.position.y > path.control_points[0].pose.position.y)
                tmp_point.pose.position.y = path.control_points[0].pose.position.y + cnt * step_size_ * s;
            else
                tmp_point.pose.position.y = path.control_points[0].pose.position.y - cnt * step_size_ * s;
            
            tmp_point.curvature = 0.0f;
            float dis_to_start = std::hypotf(tmp_point.pose.position.x-path.key_points[0].pose.position.x, tmp_point.pose.position.y-path.key_points[0].pose.position.y);
            float dis_to_end   = std::hypotf(tmp_point.pose.position.x-path.key_points[1].pose.position.x, tmp_point.pose.position.y-path.key_points[1].pose.position.y);
            if (dis_to_start > point_id_dis_precision_ && dis_to_end > point_id_dis_precision_)
            {
                tmp_point.waypoint_id = "";
            }
            else if (dis_to_start > dis_to_end)
            {
                tmp_point.waypoint_id = path.key_points[1].pointid;
            }
            else
            {
                tmp_point.waypoint_id = path.key_points[0].pointid;
            }
            tmp_point.amr_angle = path.angle;
            tmp_point.angle_type = path.angle_type;
            // tmp_point.
            if (!RepeatDetection(tmp_point.pose, trajectory))
                trajectory.points.push_back(tmp_point);            
            cnt++;
        }
        tmp_point.pathid = path.pathid;
        tmp_point.pose.position.x = path.control_points[1].pose.position.x;
        tmp_point.pose.position.y = path.control_points[1].pose.position.y;
        float dis_to_start = std::hypotf(tmp_point.pose.position.x-path.key_points[0].pose.position.x, tmp_point.pose.position.y-path.key_points[0].pose.position.y);
        float dis_to_end   = std::hypotf(tmp_point.pose.position.x-path.key_points[1].pose.position.x, tmp_point.pose.position.y-path.key_points[1].pose.position.y);
        if (dis_to_start > point_id_dis_precision_ && dis_to_end > point_id_dis_precision_)
        {
            tmp_point.waypoint_id = "";
        }
        else if (dis_to_start > dis_to_end)
        {
            tmp_point.waypoint_id = path.key_points[1].pointid;
        }
        else
        {
            tmp_point.waypoint_id = path.key_points[0].pointid;
        }
        tmp_point.amr_angle = path.angle;
        tmp_point.angle_type = path.angle_type;
        if (!RepeatDetection(tmp_point.pose, trajectory))
            trajectory.points.push_back(tmp_point);
    }
    return trajectory;
}

amr_trajectory_msgs::amr_trajectory_msg MapOperation::CirclePro(amr_trajectory_msgs::path_info &path, std::string id)
{
    amr_trajectory_msgs::amr_trajectory_msg trajectory;
    return trajectory;
}

amr_trajectory_msgs::amr_trajectory_msg MapOperation::BsplinePro(amr_trajectory_msgs::path_info &path, std::string id)
{
    amr_trajectory_msgs::amr_trajectory_msg trajectory;
    return trajectory;    
}

void MapOperation::CalcHeading(amr_trajectory_msgs::amr_trajectory_msg &all_trajectory)
{
    if (all_trajectory.points.empty())
        return;
    
    for (std::vector<amr_trajectory_msgs::point_info>::iterator iter=all_trajectory.points.begin();iter != all_trajectory.points.end()-1;iter++)
    {
        float theta = 0;
        if (ptr_graph_->GetPathMap().find(iter->pathid) != ptr_graph_->GetPathMap().end())
        {
            if (ptr_graph_->GetPathMap()[iter->pathid]->controlpoints.size() == 2)
            {
                if (iter->pathid == (iter+1)->pathid)
                {
                    theta = atan2((iter+1)->pose.position.y-iter->pose.position.y, (iter+1)->pose.position.x-iter->pose.position.x);
                }
                else
                {
                    theta = atan2(-(iter-1)->pose.position.y + iter->pose.position.y, -(iter-1)->pose.position.x+iter->pose.position.x);
                }
            }
            iter->pose.orientation = tf::createQuaternionMsgFromYaw(theta);            
        }
        else
        {
            throw("Error path id %d\n", iter->pathid);
        }
    }

    all_trajectory.points.back().pose.orientation = all_trajectory.points[all_trajectory.points.size()-2].pose.orientation;
}

bool MapOperation::RepeatDetection(geometry_msgs::Pose new_point, amr_trajectory_msgs::amr_trajectory_msg &trajectory)
{
    if (trajectory.points.size() == 0)
    {
        return false;
    }
    float dis = std::hypot(new_point.position.x-trajectory.points[trajectory.points.size()-1].pose.position.x, new_point.position.y-trajectory.points[trajectory.points.size()-1].pose.position.y);
    if (dis < 0.001)
        return true;
    else
        return false;
}

void MapOperation::GenerateViewer(void)
{
    // visualization_msgs::Marker point;
    // msg_vis_map_.markers.clear();
    // int i = 0;
    // for (auto p: all_trajectory_.points)
    // {
    //     point.header.frame_id = "global";
    //     point.type = visualization_msgs::Marker::CUBE;
    //     point.header.stamp = ros::Time::now();
    //     point.lifetime = ros::Duration();
    //     point.frame_locked = true;
    //     point.action = visualization_msgs::Marker::ADD;
    //     point.frame_locked = true;
    //     point.ns = "";
    //     point.id = i++;

    //     point.pose = p.pose;

    //     point.scale.x = 0.1;
    //     point.scale.y = 0.1;
    //     point.scale.z = 0.1;

    //     if (fabs(p.target_speed) < 0.001) //0
    //     {
    //         point.color.r = 1.0f;
    //         point.color.g = 1.0f;
    //         point.color.b = 1.0f;
    //         point.color.a = 1.0;
    //     }
    //     else if (fabs(p.target_speed) < 0.5) //0-1
    //     {
    //         point.color.r = 0.0f;
    //         point.color.g = 1.0f;
    //         point.color.b = 1.0f - p.target_speed;
    //         point.color.a = 1.0;
    //     }
    //     else
    //     {
    //         if (fabs(p.target_speed) > 1) //>2
    //         {
    //             point.color.r = 1.0f;
    //             point.color.g = 0.0f;
    //             point.color.b = 0.0f;
    //             point.color.a = 1.0;
    //         }
    //         else //1-2
    //         {
    //             point.color.r = p.target_speed - 0.5;
    //             point.color.g = 1.0f - p.target_speed;
    //             point.color.b = 0.0f;
    //             point.color.a = 1.0;
    //         }
    //     }

    //     if (i == all_trajectory_.points.size())
    //     {
    //         point.color.r = 1.0f;
    //         point.color.g = 0.0f ;
    //         point.color.b = 0.0f;
    //         point.color.a = 1.0;
	// 		point.scale.x = 0.3;
    //     	point.scale.y = 0.3;
    //     	point.scale.z = 0.1;
    //     }
    //     msg_vis_map_.markers.push_back(point);

    //     pcl::PointXYZL point;
    //     point.x = p.pose.position.x;
    //     point.y = p.pose.position.y;
    //     point.z = p.pose.position.z;
        
    //     point.label = std::atoi(p.pathid.c_str());
    //     ptr_points_->points.push_back(point);
    // }
    // printf("points size: %d, %d\n", (int)msg_vis_map_.markers.size(), (int)ptr_points_->points.size());
}

void MapOperation::PublishViewer()
{
    if (pub_vis_map_.getNumSubscribers() < 1)
    {
        ROS_WARN_ONCE("Please create a subscriber to the marker");
        return;
    }

    static int cnt = 0;
    if (cnt < 5)
    {
        pub_vis_map_.publish(all_trajectory_);
        cnt ++;
    }
}

bool MapOperation::GetPathInfoService(map_operation_msgs::GetPathInfoRequest &req, map_operation_msgs::GetPathInfoResponse &resp)
{
    resp.is_valid = false;
    if (req.mode == 0)
    {
        if (ptr_graph_->GetPathMap().find(req.pathid) != ptr_graph_->GetPathMap().end())
        {
            resp.is_valid = true;
            resp.rpathid = ptr_graph_->GetPathMap()[req.pathid]->pathid;
            resp.max_speed = ptr_graph_->GetPathMap()[req.pathid]->maxspeed;
            resp.pathtype = ptr_graph_->GetPathMap()[req.pathid]->pathtype;
            
            amr_trajectory_msgs::attrpoints point;
            point.pointid = ptr_graph_->GetPathMap()[req.pathid]->start;
            point.pose.position.x = ptr_graph_->GetPointMap()[point.pointid]->x;
            point.pose.position.y = ptr_graph_->GetPointMap()[point.pointid]->y;
            point.pose.position.z = ptr_graph_->GetPointMap()[point.pointid]->z;
            point.pose.orientation = tf::createQuaternionMsgFromYaw(0.0f);
            resp.control_points.push_back(point);

            point.pointid = ptr_graph_->GetPathMap()[req.pathid]->end;
            point.pose.position.x = ptr_graph_->GetPointMap()[point.pointid]->x;
            point.pose.position.y = ptr_graph_->GetPointMap()[point.pointid]->y;
            point.pose.position.z = ptr_graph_->GetPointMap()[point.pointid]->z;
            point.pose.orientation = tf::createQuaternionMsgFromYaw(0.0f);
            resp.control_points.push_back(point);

            resp.key_points = resp.control_points;
            resp.direction = "start2end"; // TODO
            resp.angle = ptr_graph_->GetPathMap()[req.pathid]->amr_angle;
            resp.angle_type = ptr_graph_->GetPathMap()[req.pathid]->angle_type;
        }
    }
    else if (req.mode == 1)
    {
        for (auto path : ptr_graph_->GetPathMap())
        {
            if ( (path.second->start == req.start && path.second->end == req.end) || (path.second->start == req.end && path.second->end == req.start))
            {
                resp.is_valid = true;
                resp.rpathid  = path.first;
                resp.max_speed = ptr_graph_->GetPathMap()[resp.rpathid]->maxspeed;
                resp.pathtype = ptr_graph_->GetPathMap()[resp.rpathid]->pathtype;
            
                amr_trajectory_msgs::attrpoints point;
                point.pointid = ptr_graph_->GetPathMap()[resp.rpathid]->start;
                point.pose.position.x = ptr_graph_->GetPointMap()[point.pointid]->x;
                point.pose.position.y = ptr_graph_->GetPointMap()[point.pointid]->y;
                point.pose.position.z = ptr_graph_->GetPointMap()[point.pointid]->z;
                point.pose.orientation = tf::createQuaternionMsgFromYaw(0.0f);
                resp.control_points.push_back(point);

                point.pointid = ptr_graph_->GetPathMap()[resp.rpathid]->end;
                point.pose.position.x = ptr_graph_->GetPointMap()[point.pointid]->x;
                point.pose.position.y = ptr_graph_->GetPointMap()[point.pointid]->y;
                point.pose.position.z = ptr_graph_->GetPointMap()[point.pointid]->z;
                point.pose.orientation = tf::createQuaternionMsgFromYaw(0.0f);
                resp.control_points.push_back(point);

                resp.key_points = resp.control_points;
                resp.direction = "start2end"; 
                resp.angle = ptr_graph_->GetPathMap()[resp.rpathid]->amr_angle;
                resp.angle_type = ptr_graph_->GetPathMap()[resp.rpathid]->angle_type;
                break;
            }
        }
    }
    LINFO("GetPathInfoService() --> Valid: {}, mode: {}, QPathid: {}, Start: {}, End: {}, RPathid: {}", 
           resp.is_valid, req.mode, req.pathid, req.start, req.end, resp.rpathid);
    
    // printf("GetPathInfoService() --> Valid: %d, mode: %d, QPathid: %s, Start: %s, End: %s, RPathid: %s\n", 
    //        resp.is_valid, req.mode, req.pathid.c_str(), req.start.c_str(), req.end.c_str(), resp.rpathid.c_str());
    return resp.is_valid;
}

bool MapOperation::GetPointInfoService(map_operation_msgs::GetPointInfoRequest &req, map_operation_msgs::GetPointInfoResponse &resp)
{
    resp.is_valid = false;
    if (ptr_graph_->GetPointMap().find(req.pointid) != ptr_graph_->GetPointMap().end())
    {
        resp.is_valid = true;
        resp.pose.position.x = ptr_graph_->GetPointMap()[req.pointid]->x;
        resp.pose.position.y = ptr_graph_->GetPointMap()[req.pointid]->y;
        resp.pose.position.z = ptr_graph_->GetPointMap()[req.pointid]->z;
        resp.pose.orientation = tf::createQuaternionMsgFromYaw(0.0f);

        resp.fun_attribute = "";
    }
    // printf("GetPointInfoService() --> Valid: %d, id: %s\n", resp.is_valid, req.pointid.c_str());
    return resp.is_valid;
}

bool MapOperation::GetNearestInfoService(map_operation_msgs::GetNearestInfoRequest &req, map_operation_msgs::GetNearestInfoResponse &resp)
{
    resp.is_valid = false;
    float min_dis = FLT_MAX;
    for (auto path : v_path_)
    {
        if (path.pathtype == "line")
        {
            float line_theta = atan2(path.key_points[1].pose.position.y-path.key_points[0].pose.position.y, 
                                     path.key_points[1].pose.position.x-path.key_points[0].pose.position.x);

            float dis = com_.Point2LineDistanceWithSign(req.local_msg.pose.position.x, req.local_msg.pose.position.y, 
                                                        path.key_points[0].pose.position.x, path.key_points[0].pose.position.y, line_theta);

            if (fabs(dis) < req.dis_thres)
            {
                float projection_x = 0.0f;
                float projection_y = 0.0f;
                
                if (com_.Point2LineProjection(req.local_msg.pose.position.x, req.local_msg.pose.position.y,
                                              path.key_points[0].pose.position.x, path.key_points[0].pose.position.y,
                                              path.key_points[1].pose.position.x, path.key_points[1].pose.position.y,
                                              projection_x, projection_y))
                {
                    bool in_line = com_.PointInLineSegment(projection_x, projection_y,
                                                           path.key_points[0].pose.position.x, path.key_points[0].pose.position.y,
                                                           path.key_points[1].pose.position.x, path.key_points[1].pose.position.y);
                    
                    bool in_p1 = false, in_p2 = false; 
                    if (!in_line)
                    {
                        in_p1 = std::hypotf(req.local_msg.pose.position.x - path.key_points[0].pose.position.x, req.local_msg.pose.position.y - path.key_points[0].pose.position.y) < 0.3f;
                        in_p2 = std::hypotf(req.local_msg.pose.position.x - path.key_points[1].pose.position.x, req.local_msg.pose.position.y - path.key_points[1].pose.position.y) < 0.3f;
                    }
                    if (in_line || in_p1 || in_p2)
                    {
                        float d_theta = 0.0f;
                        if (path.angle_type == "turn")
                        {
                            d_theta = com_.AngleConstraintToPI(tf::getYaw(req.local_msg.pose.orientation)-line_theta);
                        }
                        else if (path.angle_type == "shift")
                        {
                            d_theta = com_.AngleConstraintToPI(tf::getYaw(req.local_msg.pose.orientation)-path.angle);
                        }
                        
                        if (fabs(d_theta) < req.theta_thres)
                        {   
                            min_dis = std::min(min_dis, dis);
                            
                            resp.pathid = path.pathid;
                            resp.is_valid = true;
                        }
                    }
                }
            }

            // printf("pathid: %s, theta: %f, dis: %f, local: %f %f, point0: %f, %f, point1: %f %f, valid: %d\n", 
            //         path.pathid.c_str(), line_theta, dis, req.local_msg.pose.position.x, req.local_msg.pose.position.y,
            //         path.key_points[0].pose.position.x, path.key_points[0].pose.position.y,
            //         path.key_points[1].pose.position.x, path.key_points[1].pose.position.y, resp.is_valid);
        }
    }
    if (resp.is_valid)
    {
        std::string point0 = ptr_graph_->GetPathMap()[resp.pathid]->keypoints[0];
        std::string point1 = ptr_graph_->GetPathMap()[resp.pathid]->keypoints[1];
        float dis0 = std::hypotf(req.local_msg.pose.position.x-ptr_graph_->GetPointMap()[point0]->x, req.local_msg.pose.position.y-ptr_graph_->GetPointMap()[point0]->y);
        float dis1 = std::hypotf(req.local_msg.pose.position.x-ptr_graph_->GetPointMap()[point1]->x, req.local_msg.pose.position.y-ptr_graph_->GetPointMap()[point1]->y);

        float dis = std::min(dis0, dis1);
        if (dis < 0.3f)
        {
            resp.pointid = (dis0<dis1)?point0:point1;
        }
        ROS_WARN("GetNearestInfoService() --> path: %s", resp.pathid.c_str());
    }
    return resp.is_valid;
}

